// Disable Error C4996 that occur when using Boost.Signals2.
#ifdef _DEBUG
#define _SCL_SECURE_NO_WARNINGS
#endif

#include "kinect2_grabber.h"
#include <pcl/io/pcd_io.h>
#include <vector>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/visualization/pcl_visualizer.h>
typedef pcl::PointXYZRGBA PointType;
int m = 0;
bool new_cloud_;
int counter = 0;
boost::mutex mutex;
pcl::PointCloud<PointType>::ConstPtr cloud, cloud_src;
pcl::PointCloud<PointType>::Ptr c_s(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr c_f(new pcl::PointCloud<PointType>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass_(new pcl::PointCloud<pcl::PointXYZ>), cloud_pass_downsampled_(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI, pcl::Normal> harris;
pcl::UniformSampling<PointType> uniform_sampling;

void cloud_cb(const pcl::PointCloud<PointType>::ConstPtr &c)
{
	boost::mutex::scoped_lock lock(mutex);
	cloud_src = c->makeShared();
	c_s = c->makeShared();

	/*
	cloud_pass_downsampled_.reset(new pcl::PointCloud<pcl::PointXYZ>);
	uniform_d(c_s, *c_f);  
	filter(c_s, *c_f);
	harris3D(c_f, *cloud_pass_downsampled_);
	new_cloud_ = true;
	*/
}

void keyboard_func(const pcl::visualization::KeyboardEvent &event)
{
	if (event.getKeyCode() == VK_SPACE && event.keyDown()){
		boost::mutex::scoped_try_lock lock(mutex);
		if (lock.owns_lock()){
			std::cout << "111" << endl;
			pcl::io::savePCDFile("cloud.pcd", *cloud_src, true); // Binary format
			//pcl::io::savePCDFile( "cloud.pcd", *cloud, false ); // ASCII format
		}
	}
}

int main3( int argc, char* argv[] )
{
    // PCL Visualizer
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
        new pcl::visualization::PCLVisualizer( "Point Cloud Viewer" ) );
    viewer->setCameraPosition( 0.0, 0.0, -2.5, 0.0, 0.0, 0.0 );

    boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function =
		boost::bind(&cloud_cb, _1);

    // Kinect2Grabber
    boost::shared_ptr<pcl::Grabber> grabber = boost::make_shared<pcl::Kinect2Grabber>();

    // Register Callback Function
    boost::signals2::connection connection = grabber->registerCallback( function );

	
	// Keyboard Callback Function
    boost::function<void( const pcl::visualization::KeyboardEvent& )> keyboard_function =
		boost::bind(&keyboard_func, _1);
		
    // Register Callback Function
    viewer->registerKeyboardCallback( keyboard_function );

    // Start Grabber
    grabber->start();

    while( !viewer->wasStopped() ){
        // Update Viewer
        viewer->spinOnce();
		m++;
        boost::mutex::scoped_try_lock lock( mutex );
        if( lock.owns_lock() && cloud_src ){
            // Update Point Cloud
			if (!viewer->updatePointCloud(cloud_src, "cloud")){
				viewer->addPointCloud(cloud_src, "cloud");
            }
        }
    }

    // Stop Grabber
    grabber->stop();
    
    // Disconnect Callback Function
    if( connection.connected() ){
        connection.disconnect();
    }

    return 0;
}
